#ifndef _PERIPHERAL_FORK_H__
#define _PERIPHERAL_FORK_H__

/* @FileName:peripheral.h
 * @Author: haibo.yang
 * @Date: 2022-09-1  08:55:14
 * @Last Modified by: haibo.yang
 * @Last Modified time: 2022-09-1 11:17:39
 */
#pragma once

#include "rclcpp/rclcpp.hpp"
// #include <dynamic_reconfigure/server.h>
#include <sensor_msgs/msg/joint_state.hpp>
// #include <canopen_chain_node/SetObject.h>
#include <agv_actions/action/peripheral.h>
#include <agv_msgs/msg/action.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/periph_teleop.hpp>
#include <agv_msgs/msg/periph_custom_operation.hpp>
#include <agv_msgs/msg/agv_mode.hpp> // 2023.12.26
#include <agv_peripheral_controller/peripheral_event.h>
#include <agv_msgs/msg/load.hpp>
#include <agv_msgs/msg/drivers_info.hpp>
#include <agv_msgs/msg/set_stop_bits.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <vector>
#include <controller_manager_msgs/srv/list_controllers.hpp>
#include <controller_manager_msgs/msg/controller_state.hpp>
#include <yaml-cpp/yaml.h>
#include <thread>
#include <mutex>
#include <fstream>

#include <agv_peripheral_controller/fork/fork_controller.h>

#define FORKLIFT_JOINT  "forkLift_joint"
#define ZAPI_JOINT      "steer2sd_wheel_joint"
#define OPERATE_TIMEOUT_S (10)
#define MOVE_TIMEOUT_S (60)
#define PACKAGE_NAME "agv_peripheral_controller"
#define FILE_NAME "/params/posRecord.yaml"


#define AGV_PERIPHERAL_CONTROLLER_NODE_ID   (2)
#define AGV_PERIPHERAL_ForkTipCheck   (3)


namespace agv_peripheral
{

  class ForkController;
  class ChargeController;
  
  class AgvPeripheral
  {
  public:
    enum Type
    {
      JACKING,
      ROTARY,
      JACKING_ROTARY,
      ROLLER,
      FORKLIFT,
      ZAPI_CART,
      YUFENG_CART,
      MULTI_JACK,
    } peripheral_type;

    AgvPeripheral(const std::string &name);

    ~AgvPeripheral();

    bool init();
    void spin();
    void shutdown() { shutdown_req = true; };
    void preemptCB();
    void goalCB();
    void periphStateMachineUpdate();
    void periphStatusGather();
    bool periphMotorErrCheck();
    bool periphMotorEnableCheck();
    bool periphHomingCheck();
    bool periphReadyCheck();
    bool periphGetControllerRunningStatus();
  protected:
    rclcpp::Node nh; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
    actionlib::SimpleActionServer<agv_actions::PeripheralAction> as;
    agv_actions::PeripheralFeedback feedback;
    agv_actions::PeripheralResult result;


  private:
    std::string name;
    double frequency;
    bool shutdown_req;
    bool load_with_qrcode;
    bool find_joint_id;
    int joint_id;
    float max_pos;  //unit is mm or rads
    float min_pos;  //unit is mm or rads
    float target_pos;
    float previous_pos;
    float joint_pos;
    float file_record_pos;
    bool read_record_result;
    bool periphInitialized;
    // int jacking_speed;
    std::string pos_record_file;
    rclcpp::Time operate_time;
    rclcpp::Time jointState_update_time;

    
    std::string action_pick = "pick";
    std::string action_drop = "drop";
    std::string action_adjust = "lhdAdjust";
    rclcpp::Time last_odom_time;

    bool load_status;
    bool enable_package_detect;
    bool post_check_enable;
    bool ForkTip_enable;
    bool shielded_cargo_state_detection;
    double distance;
    double forklift_manual_acc;
    double forklift_manual_vel;
    double drop_detect_distance;
    int  up_comp;
    int  down_comp;

    int current_forklift_homestate = -1;
    int last_forklift_homestate = -1;

    bool forklift_move_state = false;
    bool forklift_arrive_recordPos = false;

    bool forklift_manual_active_up = false;
    bool forklift_manual_active_down = false;
    rclcpp::Time forklift_manual_start_time_up;
    rclcpp::Time forklift_manual_start_time_down;
    rclcpp::Time forklift_manual_stop_time_up;
    rclcpp::Time forklift_manual_stop_time_down;
    bool forklift_manual_stop_flag_up = false;
    bool forklift_manual_stop_flag_down = false;
    double forklift_timeout_time;
    rclcpp::Time last_forklift_time;

    rclcpp::Time last_set_stop_time;

    /*** global variables ***/
    sensor_msgs::msg::JointState joint_state; /**< joint state */
    std::mutex m_mutex;
    std::mutex m_mutex1;
    float manual_target_vel;      /**< manual ctl mode position */
    std::vector<agv_msgs::msg::PeriphMotorInfo> periphMotor;
    agv_msgs::msg::PeriphStatus periphStatus;
    agv_msgs::msg::Load loadInfo;
    std::vector<agv_msgs::msg::Load> loads;
    agv_msgs::msg::DriversInfo driversInfo;
    agv_msgs::msg::SetStopBits setStop;
    agv_msgs::msg::OdometryLite odom_data;
    std_srvs::srv::Trigger trig_srv;
    agv_msgs::msg::PoseWithConfidence current_pose;
    agv_msgs::msg::Action current_action;

    agv_msgs::msg::AgvAutoStatus agv_status;

    std::thread m_thread;
    std::timed_mutex m_quit_mutex;

    ros::Subscriber js_sub;    /**< joint state from hardware interface */

    ros::ServiceClient PeripheralHaltCtl; /**< joint point halt contrl */
    ros::ServiceClient PeripheralRecoverCtl; /**< joint point recover contrl */
    ros::ServiceClient peripheralGetcontrollers;

    ros::Publisher  periph_status_pub;
    ros::Subscriber periph_teleop_sub;
    ros::Subscriber agvmode_sub;  // 2023.12.26
    ros::Publisher  load_pub;
    ros::Subscriber driver_info_sub;
    ros::Publisher  set_stop_pub;
    ros::Subscriber auto_status_sub;
    ros::Subscriber readinput_sub;
    ros::Subscriber odom_sub;
    ros::Subscriber pose_sub;

    // ZAPI
    ros::Publisher  custom_operation_pub;
    ros::Subscriber zapi_di_sub;

    /*********************
    ** Dynamic Reconfigure
    **********************/
    // dynamic_reconfigure::Server<agv_peripheral_controller::periphDynParamConfig> dynamic_reconfigure_server;
    // dynamic_reconfigure::Server<agv_peripheral_controller::periphDynParamConfig>::CallbackType dynamic_reconfigure_callback;


    ForkController*   fork_controller;

    PeripheralEvent pevent;

    // void reconfigCB(agv_peripheral_controller::periphDynParamConfig &config, uint32_t unused_level);
    void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &ms);
    bool executeOP(int order);
    void periphTeleopCtlcb(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg);
    void agvModecb(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg); // 2023.12.26 modify by haibo
    void driversInfoCallback(const agv_msgs::msg::DriversInfo::ConstSharedPtr &msg);
    void autoStatusCallback(const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr &msg);
    void readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg);
    void odomCallback(const agv_msgs::msg::OdometryLite::ConstSharedPtr &msg);
    void poseCallback(const agv_msgs::msg::PoseWithConfidence::ConstSharedPtr &msg);
    static void thread_fun(AgvPeripheral *pThis);
    void thread_cycle();
    void periphPosRecord();
    bool periphRecordPosRead();
    bool periphRecordPosWrite();
    bool periphMoveToRecordPos();
    void check_jointState_out();
    void set_stop_bits();
    void drop_post_check(bool enable);
    void ForkTipCheck();
    void zapiDiCallback(const std_msgs::msg::UInt16::ConstSharedPtr &msg);
  
    enum periph_op_enum
    {
      NO_OP,
      UP,
      DOWN,
      LIFT,
    };

    enum periph_stata_enum
    {
      UNCONTROLLED_STATE = -1,
      IDLE_STATE,
      FORKLIFT_OP_STATE,
      FORKLIFT_SUCCESS_STATE,
      MANUAL_MODE_STATE = 7,
      HALT_HANDLE_STATE,
    };

    enum periph_result_enum
    {

      SUCCESS,
      FAILED,
      CANCELED,
      OTHER,
    };


    struct OperateState
    {
      int cur_state;
      int cur_op;
      int next_op;
    } os;

    struct peripheral_motor_status
    {
      bool enableState;
      bool homeState;
      bool motorErr;
      bool movingState;
      bool ready;
    }motorStatus[2];

    struct periph_input_status
    {
      bool manualState;
    }input_status;

    struct periph_read_input
    {
      bool reset;
      bool emergency;
      bool upper_limit;
      bool lower_limit;
      bool loadStatus;
      bool leftTipSensor;
      bool rightTipSensor;
      bool leftTipTouchSensor;
      bool rightTipTouchSensor;      
      bool leftLoadInplace;
      bool rightLoadInplace;
      bool upLimit;
      bool downLimit;
      bool forkLiftUpLimit;
    }read_input_data, last_read_input_data;

    struct zapi_cart_status
    {
      int  current_operation;
      bool upper_limit;
      bool lower_limit;
    }zapi_cart_data;

    struct yufeng_cart_status
    {
      int current_operation;
    }yufeng_cart_data;
    
  public:
    void set_cur_op(periph_op_enum op);
    int get_cur_op();
    void set_next_op(periph_op_enum op);
    int get_next_op();
    void set_cur_state(periph_stata_enum op);
    int get_cur_state();
    void clear_os();


  };
} // agv_peripheral

#endif // _PERIPHERAL_FORK_H__
